/**
 *  sem.c - Semaphore Mechanism.
 *
 *  Copyright (C) 2008-2009 ZhangHu
 *  All rights reserved.
 *  E-MAIL: anmnmnly@gmail.com
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "include/sem.h"


static list_t sem_head; /* pointer to the head and rear of semaphore queue */



/**
 * initsem - Initialize sem_head.
 * @return:
 *
 * @notes:
 */
void initsem(void) {
    sem_head.next = &sem_head;
    sem_head.prev = &sem_head;
}



/**
 * check_id - Check the ID is whether existed.
 * @semid: The semaphore ID that need to check out.
 * @return: Semaphore or NULL.
 *
 * @notes: If the ID has existed in semaphore queue, and then return
 *         semaphore's address, otherwise return NULL.
 */
static sem_t *check_id(semid_t semid) {
    sem_t *psem = NULL;
    list_t *plist = NULL;

    if((sem_head.next == &sem_head) ||
        (sem_head.prev == &sem_head))
    {
        return(NULL);
    }

    plist = sem_head.next;
    do {
        psem = mac_find_entry(plist, sem_t, sem_link);
        if(semid == psem->sem_id) {
            return(psem);
        } else {
            plist = plist->next;
        }
    } while(plist != sem_head.next);

    return(NULL);
}



/**
 * rdy_to_sem_wait - Move TCB from ready queue to semaphore waiting queue.
 * @psem: Pointer to semaphore.
 * @ptcb: Pointer to TCB.
 * @return:
 *
 * @notes:
 */
static void rdy_to_sem_wait(sem_t *psem, tcb_t *ptcb) {
    uword_t prio = 0;
    list_t *del_tcb = NULL;

    if((psem == NULL) || (ptcb == NULL)) {
        return;
    }

    prio = ptcb->prio;
    del_tcb = del_tcb_rdy(ptcb);
    if(del_tcb != NULL) {
        add_node_list_rear(&psem->task_link[prio], del_tcb);
    }
}



/**
 * sem_wait_to_rdy - Move TCB form semaphore waiting queue to ready queue
 * @psem: Pointer to semaphore.
 * @del_tcb: Pointer to TCB.
 * @return:
 *
 * @notes:
 */
static void sem_wait_to_rdy(sem_t *psem, list_t *del_tcb) {
    list_t *plist = NULL;
    tcb_t *ptcb = NULL;

    if((psem == NULL) || (del_tcb == NULL)) {
        return;
    }

    ptcb = mac_find_entry(del_tcb, tcb_t, link);
    plist = del_node_list(&psem->task_link[ptcb->prio], del_tcb);
    ptcb = mac_find_entry(plist, tcb_t, link);
    add_tcb_rdy(ptcb);
}



/**
 * find_max_prio_tcb - Find the highest priority task in semaphore waiting queue
 *                     and return the link_address.
 * @psem: Pointer to semaphore.
 * @return: The 'link' member of TCB. otherwise return NULL.
 *
 * @notes:
 */
static list_t *find_max_prio_tcb(sem_t *psem) {
    uword_t i = 0;

    if(psem == NULL) {
        return(NULL);
    }

    for(i = 0; i < MAX_PRIO; i++) {
        if(psem->task_link[i].next !=  &psem->task_link[i]) {
            return psem->task_link[i].next;
        }
    }

    return(NULL);
}



/* The following functions are application program interface */

/**
 * semInit - Creating a semaphore.
 * @SemID: The ID of new semaphore.
 * @InitCnt: The initial count value of new semaphore.
 * @err: Return reasons about this operation.
 * @return: The entry of new semaphore.
 *
 * @notes:
 */
sem_t *semInit(semid_t SemID, uword_t InitCnt) {
    uword_t i = 0;
    sem_t *psem = NULL;

    psem = (sem_t*)kmalloc(sizeof(sem_t));

    if(psem == NULL) {
        return(NULL);
    }

    mac_disable_irq();
    if(check_id(SemID) == NULL) {
        psem->sem_link.next = &psem->sem_link;
        psem->sem_link.prev = &psem->sem_link;
        psem->sem_id = SemID;
        psem->sem_cnt = InitCnt;
        psem->task_cnt = 0;
        psem->tcb_link.next = NULL;

        for(i = 0; i < MAX_PRIO; i++) {
            psem->task_link[i].next = &psem->task_link[i];
            psem->task_link[i].prev = &psem->task_link[i];
        }
        add_node_list_rear(&sem_head, &psem->sem_link);
        mac_enable_irq();
        return(psem);
    } else {
        mac_enable_irq();
        kfree(psem);
        return(NULL);
    }
}



/**
 * semInitMutex - Creating a mutex semaphore.
 * @SemID: The ID of new semaphore.
 * @return: The entry of new semaphore, otherwise NULL for failed.
 *
 * @notes:
 */
sem_t *semInitMutex(semid_t SemID) {
    return(semInit(SemID, 1));
}



/**
 * semGain - Get semaphore, if operating is failed,
 *           the task will be block, and its TCB will be
 *           deleted from ready queue, and then insert it
 *           into waiting queue of semaphore.
 * @SemID: The ID of semaphore.
 * @pSem: Pointer to semaphore.
 * @return:
 *
 * @notes: SemID or pSemaphore is valide to specify a semaphore.
 */
void semGain(semid_t SemID, sem_t *pSemaphore) {
    sem_t *psem = NULL;

    mac_disable_irq();

    /* Get the real entry of semaphore */
    psem = pSemaphore;
    if(psem == NULL) {
        psem = check_id(SemID);
        if(psem == NULL) {
            mac_enable_irq();
            return;
        }
    }

    /* semaphore is available */
    if(psem->sem_cnt > 0) {
        psem->sem_cnt--;
        add_node_single_list_head(&(current()->sem_link),
                                  &psem->tcb_link);
        mac_enable_irq();
        return;
    }

    /* semaphore is unavailable, so current task blocked. */
    current()->status = T_BLOCK;
    psem->task_cnt++;
    rdy_to_sem_wait(psem, current());
    mac_enable_irq();
    scheduler();        /* other task have a opportunity to run. */
}



/**
 * semGainFree - Get semaphore, if the operating is failed,
 *               the task should not be blocked.
 * @SemID: The ID of semaphore.
 * @pSem: Pointer to semaphore.
 * @return:
 *
 * @notes:
 */
bool_t semGainFree(semid_t SemID, sem_t *pSemaphore) {
    sem_t *psem = NULL;

    mac_disable_irq();

    psem = pSemaphore;
    if(psem == NULL) {
        psem = check_id(SemID);
        if(psem == NULL) {
            mac_enable_irq();
            return FALSE;
        }
    }

    if(psem->sem_cnt > 0) {
        psem->sem_cnt--;
        add_node_single_list_head(&(current()->sem_link),
                                  &psem->tcb_link);
        mac_enable_irq();
        return TRUE;
    }

    mac_enable_irq();
    return FALSE;
}



/**
 * semPost - Unlock a semaphore.
 * @SemID: The ID of semaphore.
 * @pSem: Pointer to semaphore.
 * @return: Return results about this operation.
 *
 * @notes:
 */
void semPost(semid_t SemID, sem_t *pSemaphore) {
    sem_t *psem = NULL;
    sem_t *sem = NULL;
    list_t *plist = NULL;
    slist_t *slist = NULL;
    tcb_t *ptcb = NULL;

    mac_disable_irq();

    psem = pSemaphore;
    if(psem == NULL) {
        psem = check_id(SemID);
        if(psem == NULL) {
            mac_enable_irq();
            return;
        }
    }

    slist = &(current()->sem_link);
    slist = slist->next;
    while(slist != NULL) {
        sem = mac_find_entry(slist, sem_t, tcb_link);
        if(sem == psem) {
            del_node_single_list(&(current()->sem_link), slist);
            break;
        }
        slist = slist->next;
    }

    if(slist == NULL) {
        mac_enable_irq();
        return;
    }

    psem->sem_cnt++;
    if(psem->sem_cnt <= 0) {
        mac_enable_irq();
        return;
    }

    plist = find_max_prio_tcb(psem);
    if(plist == NULL) {

        mac_enable_irq();
        return;
    }

    psem->task_cnt--;
    ptcb = mac_find_entry(plist, tcb_t, link);
    add_node_single_list_head(&ptcb->sem_link, &psem->tcb_link);
    psem->sem_cnt--;
    ptcb->status = T_READY;
    sem_wait_to_rdy(psem, plist);

    mac_enable_irq();
    scheduler();
    return;
}



/**
 * semDel - Deleting a semaphore.
 * @SemID: The ID of semaphore.
 * @pSem: Pointer to semaphore.
 * @return: Return results of this operation.
 *
 * @notes:
 */
void semDel(semid_t SemID, sem_t *pSemaphore) {
    uword_t i = 0;
    sem_t *psem = NULL;
    sem_t *sem = NULL;
    list_t *plist = NULL;
    slist_t *slist = NULL;
    tcb_t *ptcb = NULL;


    mac_disable_irq();

    psem = pSemaphore;
    if(psem == NULL) {
        psem = check_id(SemID);
        if(psem == NULL) {
            mac_enable_irq();
            return;
        }
    }

    // if current task gained this semophore.
    slist = &(current()->sem_link);
    slist = slist->next;
    while(slist != NULL) {
        sem = mac_find_entry(slist, sem_t, tcb_link);
        if(sem == psem) {
            del_node_single_list(&(current()->sem_link), slist);
            break;
        }
        slist = slist->next;
    }

    /* move all tasks in waiting queue to sleep queue. */
    for(i=0; i<MAX_PRIO; i++) {
        do {
            plist = psem->task_link[i].next;
            if(plist == (&psem->task_link[i])) {
                break;
            } else {
                del_node_list(&psem->task_link[i], plist);
                ptcb = mac_find_entry(plist, tcb_t, link);
                ptcb->status = T_SLEEP;
                add_tcb_sleep(ptcb);
            }
        } while(1);
    }

    del_node_list(&sem_head, &psem->sem_link);
    mac_enable_irq();
    kmemset(psem, 0, sizeof(sem_t));
    kfree(psem);

    return;
}



/**
 * release_sem - Unlock all semaphores that a task locked.
 * @ptcb: Pointer to a TCB.
 * @return:
 *
 * @notes:
 */
void release_sem(tcb_t *ptcb) {
    sem_t *psem = NULL;
    slist_t *slist = NULL;

    slist = &ptcb->sem_link;
    slist = slist->next;
    while(slist != NULL) {
        psem = mac_find_entry(slist, sem_t, tcb_link);
        semPost(0, psem);
        slist = slist->next;
    }
}

